//
// Created by kevin on 2020/5/7.
//
#include "obeject_detect/calcDepth.h"
using namespace std;
clock_t start,en;
double fx=150.0;
double fy=150.0;
double cx= 261.5;
double cy=149.5;
double b= -6.36812225e-02;
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud;
ros::Publisher pub_pc;
ros::Subscriber depMap_sub;
void depmapCallback(const std_msgs::Float32MultiArray::ConstPtr &msg){
    pcl::PointCloud<pcl::PointXYZI>::Ptr out (new pcl::PointCloud<pcl::PointXYZI>);
    int cnt = 0;
    float point_now;
    for (int v = 0; v < 300; v++){
        for (int u = 0; u < 300; u++) {
            point_now = msg->data[cnt];
            cnt++;
            if (point_now <= 0.0 || point_now >= 96.0)
            {
                continue;
            }

            double x = (u - cx) / fx;
            double y = (v - cy) / fy;
            double depth = fx* b/ (point_now);
            if(depth<-5|| depth > 5 )
                continue;
            pcl::PointXYZI point ;
            point.x = -x;
            point.y = y;
            point.z = depth;
            point.intensity = 0;//left.at<uchar>(v, u) / 255.0;
            out->push_back(point);
        }
    }
    sensor_msgs::PointCloud2 cloud;
    pcl::toROSMsg(*(out), cloud);
    ros::Time time=ros::Time::now();
    cloud.header.stamp = time;
    cloud.header.frame_id = "map";
    pub_pc.publish(cloud);
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "dis2dep");
    ros::NodeHandle nh;
    pub_pc = nh.advertise<sensor_msgs::PointCloud2>("/bi2pc/camera/PointCloud", 10);
    depMap_sub = nh.subscribe<std_msgs::Float32MultiArray>("/uav/dep/raw",1,depmapCallback);
    while(!ros::isShuttingDown()){
        ros::spinOnce();
    }
    return 0;
}



